'''
Created on Aug 5, 2019

@author: modys
'''
import numpy as np
import matplotlib.pyplot as plt
import math

class Bicycle():
    '''
    classdocs
    '''

    def __init__(self,time_end_s,sample_time_s):
        '''
        Constructor
        '''
        self.xc_m = 0.0;
        self.yc_m = 0.0;
        self.theta_rad = 0.0;
        self.delta_rad = 0.0;
        self.beta_rad = 0.0;
        
        self.L_m = 0.0;
        self.lr_m = 0.0;
        self.w_max_rad_s = 0.0;
        
        self.sample_time_s = sample_time_s;
        
        self.t_data = np.arange(0,time_end_s,sample_time_s);
        self.x_data = np.zeros_like(self.t_data);
        self.y_data = np.zeros_like(self.t_data);
        self.theta_data = np.zeros_like(self.t_data);
        self.delta_data = np.zeros_like(self.t_data);

        
    def set_cfg(self,wheelbase_m=2,rear_wheel_base_m=1.2,max_steering_rate_rad_s=1.2):
        self.L_m = wheelbase_m;
        self.lr_m = rear_wheel_base_m;
        self.w_max_rad_s = max_steering_rate_rad_s;
        
    def set_steeringAngleRad(self,delta):
        self.delta_rad = delta;
        
    def getDataLength(self):
        return self.t_data.shape[0];
    
    def angleAdjustDeg(self,inp):
        div = int(inp/360);
        rem = inp - (360.0 * div);
        return rem;
    
    def reset(self):
        self.xc_m = 0.0;
        self.yc_m = 0.0;
        self.theta_rad = 0.0;
        self.delta_rad = 0.0;
        self.beta_rad = 0.0;
        
        self.L_m = 0.0;
        self.lr_m = 0.0;
        self.w_max_rad_s = 0.0;
        
    def step(self, v, w, i):
        max(-self.w_max_rad_s, min(w, self.w_max_rad_s))
        self.xc_m    = self.xc_m + v*np.cos(self.theta_rad +self.beta_rad)*self.sample_time_s;
        self.yc_m    = self.yc_m + v*np.sin(self.theta_rad +self.beta_rad)*self.sample_time_s;
        self.theta_rad = self.theta_rad + (v*np.cos(self.beta_rad)*np.tan(self.delta_rad)/self.L_m)*self.sample_time_s;
        self.delta_rad = self.delta_rad + w * self.sample_time_s;
        self.beta_rad  = np.arctan((self.lr_m*np.tan(self.delta_rad))/self.L_m);
        
        self.x_data[i] = self.xc_m;
        self.y_data[i] = self.yc_m;
        self.theta_data[i] = self.angleAdjustDeg(math.degrees(self.theta_rad));
        self.delta_data[i] = self.angleAdjustDeg(math.degrees(self.delta_rad));
        
        
    def plotResults(self):
        plt.figure(1)
        plt.title('Path')
        plt.axis('equal')
        plt.plot(self.x_data, self.y_data)
        plt.legend()
        plt.grid()
        
        
        plt.figure(2)
        plt.title('Theta')
        plt.plot(self.t_data, self.theta_data)
        plt.legend()
        plt.grid()
        
        
        plt.figure(3)
        plt.title('delta_rad')
        plt.plot(self.t_data, self.delta_data)
        plt.legend()
        plt.grid()
        
        plt.show()